#include "motorcontroller.h"
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include "rpi.h"


#define MC_MOTOR_0 RPI_12

#define PWM_ADDRESS 0x40

#define PWM_MODE1         0x00
#define PWM_MODE2         0x01
#define PWM_SUBADR1       0x02
#define PWM_SUBADR2       0x03
#define PWM_SUBADR3       0x04
#define PWM_ALLCALLADR    0x05
#define PWM_LED0_ON_L     0x06
#define PWM_LED0_ON_H     0x07
#define PWM_LED0_OFF_L    0x08
#define PWM_LED0_OFF_H    0x09
#define PWM_LED1_ON_L     0x0A
#define PWM_LED1_ON_H     0x0B
#define PWM_LED1_OFF_L    0x0C
#define PWM_LED1_OFF_H    0x0D
#define PWM_LED2_ON_L     0x0E
#define PWM_LED2_ON_H     0x0F
#define PWM_LED2_OFF_L    0x10
#define PWM_LED2_OFF_H    0x11
#define PWM_LED3_ON_L     0x12
#define PWM_LED3_ON_H     0x13
#define PWM_LED3_OFF_L    0x14
#define PWM_LED3_OFF_H    0x15
#define PWM_LED4_ON_L     0x16
#define PWM_LED4_ON_H     0x17
#define PWM_LED4_OFF_L    0x18
#define PWM_LED4_OFF_H    0x19
#define PWM_LED5_ON_L     0x1A
#define PWM_LED5_ON_H     0x1B
#define PWM_LED5_OFF_L    0x1C
#define PWM_LED5_OFF_H    0x1D
#define PWM_LED6_ON_L     0x1E
#define PWM_LED6_ON_H     0x1F
#define PWM_LED6_OFF_L    0x20
#define PWM_LED6_OFF_H    0x21
#define PWM_LED7_ON_L     0x22
#define PWM_LED7_ON_H     0x23
#define PWM_LED7_OFF_L    0x24
#define PWM_LED7_OFF_H    0x25
#define PWM_LED8_ON_L     0x26
#define PWM_LED8_ON_H     0x27
#define PWM_LED8_OFF_L    0x28
#define PWM_LED8_OFF_H    0x29
#define PWM_LED9_ON_L     0x2A
#define PWM_LED9_ON_H     0x2B
#define PWM_LED9_OFF_L    0x2C
#define PWM_LED9_OFF_H    0x2D
#define PWM_LED10_ON_L    0x2E
#define PWM_LED10_ON_H    0x2F
#define PWM_LED10_OFF_L   0x30
#define PWM_LED10_OFF_H   0x31
#define PWM_LED11_ON_L    0x32
#define PWM_LED11_ON_H    0x33
#define PWM_LED11_OFF_L   0x34
#define PWM_LED11_OFF_H   0x35
#define PWM_LED12_ON_L    0x36
#define PWM_LED12_ON_H    0x37
#define PWM_LED12_OFF_L   0x38
#define PWM_LED12_OFF_H   0x39
#define PWM_LED13_ON_L    0x3A
#define PWM_LED13_ON_H    0x3B
#define PWM_LED13_OFF_L   0x3C
#define PWM_LED13_OFF_H   0x3D
#define PWM_LED14_ON_L    0x3E
#define PWM_LED14_ON_H    0x3F
#define PWM_LED14_OFF_L   0x40
#define PWM_LED14_OFF_H   0x41
#define PWM_LED15_ON_L    0x42
#define PWM_LED15_ON_H    0x43
#define PWM_LED15_OFF_L   0x44
#define PWM_LED15_OFF_H   0x45
#define PWM_ALL_LED_ON_L  0xFA
#define PWM_ALL_LED_ON_H  0xFB
#define PWM_ALL_LED_OFF_L 0xFC
#define PWM_ALL_LED_OFF_H 0xFD
#define PWM_PRESCALE      0xFE
#define PWM_TEST_MODE     0xFE

#define PWM_WHO_AM_I 0x1F

#define PWM_MAXDUTY 0.1
#define PWM_MINDUTY 0.05
#define PWM_MAXTIME 4095
#define PWM_PINOFFSET 4;

void voltageToBin(double voltage, __u8& h, __u8& l)
{
  h = l = 0;
  __u16 v = (unsigned int)((voltage * (PWM_MAXDUTY - PWM_MINDUTY) + PWM_MINDUTY) * PWM_MAXTIME);

  l = 0x0FF & v;
  h = (0x0FF00 & v) >> 8;
  //printf("%04X %2X %2X\n", v, h, l);
  
}

MotorController::MotorController(int file) : I2CDevice(file, PWM_ADDRESS)
{

  __u8 val;

  writeByte(PWM_MODE1, 0x11);
  val = 121;
  //printf("%02X\n", val);
  writeByte(PWM_PRESCALE, 121);


  m_led_on_l[0]  = PWM_LED4_ON_L;
  m_led_on_h[0]  = PWM_LED4_ON_H;
  m_led_off_l[0] = PWM_LED4_OFF_L;
  m_led_off_h[0] = PWM_LED4_OFF_H;
  m_led_on_l[1]  = PWM_LED5_ON_L;
  m_led_on_h[1]  = PWM_LED5_ON_H;
  m_led_off_l[1] = PWM_LED5_OFF_L;
  m_led_off_h[1] = PWM_LED5_OFF_H;
  m_led_on_l[2]  = PWM_LED6_ON_L;
  m_led_on_h[2]  = PWM_LED6_ON_H;
  m_led_off_l[2] = PWM_LED6_OFF_L;
  m_led_off_h[2] = PWM_LED6_OFF_H;
  m_led_on_l[3]  = PWM_LED7_ON_L;
  m_led_on_h[3]  = PWM_LED7_ON_H;
  m_led_off_l[3] = PWM_LED7_OFF_L;
  m_led_off_h[3] = PWM_LED7_OFF_H;
  writeByte(PWM_MODE2, 0x04);
  for (int i = 0; i < 4; i++)
  {
    writeByte(m_led_on_l[i], 0x00);
    writeByte(m_led_on_h[i], 0x00);
  }
  setVoltage(-1, 0);
  writeByte(PWM_MODE1, 0x01);
  for (int i = 0; i < 4; i++)
  {
    writeByte(m_led_on_l[i], 0x00);
    writeByte(m_led_on_h[i], 0x00);
  }
  setVoltage(-1, 0);



  

}

void MotorController::setVoltage(const int &id, const double &v)
{
  if (id < 0)
  {
    for (int i = 0; i < 4; i++)
    {
      setVoltage(i, v);
    }
  }
  else
  {
    double vv = v;
    if (v < 0) vv = 0;
    if (v > 1) vv = 1;
    //printf("%.2f\n", vv);
    __u8 h,l;
    voltageToBin(vv, h, l);
    writeByte(m_led_on_l[id], 0x00);
    writeByte(m_led_on_h[id], 0x00);
    writeByte(m_led_off_l[id], l);
    writeByte(m_led_off_h[id], h);
  }
}

